#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl_conversions/pcl_conversions.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/make_shared.h>

#include <chrono>
#include <iomanip>
#include <sstream>
#include <mutex>
#include <memory>
#include <filesystem>

class PointCloud22Pcd : public rclcpp::Node
{
private:
    // 使用 using 声明让代码更简洁
    using PointT = pcl::PointXYZI;
    using PointCloud = pcl::PointCloud<PointT>;
    using PointCloudMsg = sensor_msgs::msg::PointCloud2;

    rclcpp::Subscription<PointCloudMsg>::SharedPtr rslidar_sub_;
    rclcpp::TimerBase::SharedPtr timer_;
    
    PointCloud::Ptr latest_cloud_;
    mutable std::mutex cloud_mutex_;
    std::string output_dir_;

    void cloud_callback(const PointCloudMsg::SharedPtr msg)
    {
        auto new_cloud = pcl::make_shared<PointCloud>();
        pcl::fromROSMsg(*msg, *new_cloud);

        std::lock_guard<std::mutex> lock(cloud_mutex_);
        latest_cloud_ = new_cloud; 
    }

    void timer_callback()
    {
        PointCloud::Ptr cloud_to_save;

        {
            std::lock_guard<std::mutex> lock(cloud_mutex_);
            cloud_to_save = latest_cloud_;
        }

        if (cloud_to_save && !cloud_to_save->empty())
        {
            auto now = std::chrono::system_clock::now();
            auto now_c = std::chrono::system_clock::to_time_t(now);
            std::stringstream ss;
            ss << "point_cloud_" << std::put_time(std::localtime(&now_c), "%Y-%m-%d_%H-%M-%S");
            std::string file_name = ss.str() + ".pcd";
            std::string full_path = output_dir_ + file_name;

            try
            {
                if (pcl::io::savePCDFileASCII(full_path, *cloud_to_save) == 0)
                {
                    RCLCPP_INFO(this->get_logger(), "成功保存pcd文件: '%s'", full_path.c_str());
                }
                else
                {
                    RCLCPP_ERROR(this->get_logger(), "保存pcd文件失败: '%s'", full_path.c_str());
                }
            }
            catch (const std::exception& e)
            {
                RCLCPP_ERROR(this->get_logger(), "保存时发生错误 '%s': %s", file_name.c_str(), e.what());
            }
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "等待保存的点云数据没有接收到或为空");
        }
    }

public:
    explicit PointCloud22Pcd(const std::string &name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "节点 %s: 已启动.", name.c_str());
        
        this->declare_parameter<std::string>("pointcloud_topic", "/rslidar_points");
        std::string topic_name = this->get_parameter("pointcloud_topic").as_string();

        this->declare_parameter<int>("save_interval_s", 10);
        int save_interval_s = this->get_parameter("save_interval_s").as_int();

        this->declare_parameter<std::string>("output_directory", "./pcd_output");
        output_dir_ = this->get_parameter("output_directory").as_string();
        
        RCLCPP_INFO(this->get_logger(), "订阅话题: %s", topic_name.c_str());
        RCLCPP_INFO(this->get_logger(), "保存间隔 (秒): %d", save_interval_s);
        RCLCPP_INFO(this->get_logger(), "保存目录: %s", output_dir_.c_str());

        if (!std::filesystem::exists(output_dir_)) {
            RCLCPP_INFO(this->get_logger(), "输出目录不存在，正在创建: %s", output_dir_.c_str());
            std::filesystem::create_directories(output_dir_);
        }
        if (output_dir_.back() != '/') {
            output_dir_ += '/';
        }

        auto qos = rclcpp::QoS(10);
        rslidar_sub_ = this->create_subscription<PointCloudMsg>(
            topic_name, qos, std::bind(&PointCloud22Pcd::cloud_callback, this, std::placeholders::_1));

        timer_ = this->create_wall_timer(
            std::chrono::seconds(save_interval_s), std::bind(&PointCloud22Pcd::timer_callback, this));
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PointCloud22Pcd>("pointcloud2_to_pcd");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}